#include "PotentiometerEncoder.h"

PotentiometerEncoder::PotentiometerEncoder(int pin, bool enableKalmanFilter)
{
    _pin = pin;
    _accumulatedPosition = 0;
    _enableKalmanFilter = enableKalmanFilter;
}

void PotentiometerEncoder::begin(int maxRawValue, unsigned char resolution)
{
    pinMode(_pin, INPUT);
    analogReadResolution(resolution); // ADC精度为8位 数值范围0-255
    _lastValue = analogRead(_pin);
    // 把初始的值赋给累计值
    _accumulatedPosition = _lastValue;
    _maxRawValue = maxRawValue;
    if (_enableKalmanFilter)
    {
        // e_mea： 测量不确定性 - 我们对测量结果的期望值有多大差异
        // e_est： 估计不确定性 - 初始值可与 e_mea 相同，因为卡尔曼滤波器会调整其值。
        // q：过程方差 - 通常是一个介于 0.001 和 1 之间的小数字 - 测量值的移动速度。建议使用 0.01。应根据需要进行调整。
        kalmanFilter = new SimpleKalmanFilter(2, 2, 0.01);
    }
}

PotentiometerEncoder::~PotentiometerEncoder()
{
    delete kalmanFilter;
}

int PotentiometerEncoder::getCurrentPosition()
{
    int currentValue = analogRead(_pin);

    if (_enableKalmanFilter)
    {
        currentValue = kalmanFilter->updateEstimate(currentValue);
    }

    return currentValue;
}

long PotentiometerEncoder::getAccumulatedPosition()
{
    return _accumulatedPosition;
}

void PotentiometerEncoder::resetPosition(int32_t position)
{
    _accumulatedPosition = position;
}

int PotentiometerEncoder::getRevolutions()
{
    int p = _accumulatedPosition >> 8;
    return p;
}

float PotentiometerEncoder::getSpeed()
{
    static uint32_t timeLast = 0;
    static int positionLast = 0;

    uint32_t timeNow = micros();
    int positionCur = this->getCurrentPosition();
    uint32_t deltaT = timeNow - timeLast;
    int deltaA = positionCur - positionLast;

    // 假设两次测量之间的旋转不超过180°
    // 要求每一周至少两次测量（最好测4次）
    if (deltaA > _maxRawValue / 2)
        deltaA -= _maxRawValue;
    if (deltaA < -_maxRawValue / 2)
        deltaA += _maxRawValue;
    float speed = (deltaA * 1e6) / deltaT;

    timeLast = timeNow;
    positionLast = positionCur;

    return speed * 60.0 / _maxRawValue;
}

void PotentiometerEncoder::update()
{
    int currentValue = this->getCurrentPosition();

    // whole rotation CW
    if (_lastValue > _maxRawValue / 2 && currentValue < (_lastValue - _maxRawValue / 2))
    {
        _accumulatedPosition = _accumulatedPosition + _maxRawValue - _lastValue + currentValue;
    }
    // whole rotation CCW
    else if (currentValue > _maxRawValue / 2 && _lastValue < (currentValue - _maxRawValue / 2))
    {
        _accumulatedPosition = _accumulatedPosition - _maxRawValue - _lastValue + currentValue;
    }
    else
    {
        _accumulatedPosition = _accumulatedPosition - _lastValue + currentValue;
    }
    _lastValue = currentValue;
    return;
}
